/**
*  \file input.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "input.h"
#include "../assertor/assertor.h"
#include "../pal/pal_mem.h"
#include "../mcu/mcu.h"
#include "../interpol/interpol_s16_s16.h"
#include "../interpol/interpol_u16_u16.h"
#include "../interpol/interpol_u16_s16.h"
#include "../analog_controller/analog_controller.h"
#include "../keytime/keytime.h"
#include "../parameter/parameter.h"
#include "../cancom/cancom.h"
#include "../driver_present/driver_present.h"
#include "../mlib/middle.h"
#include "../specs/specs.h"
#include "../output/output.h"
#include "math.h"
#include "../app/app.h"
#include "../control_traction/traction.h"
/*===============================================[ private defines   ]================================================*/
S16 double_pedal_function( struct input_data_obj *p_data,F32 forward_vol_1,F32 forward_vo2,F32 backward_vol_1,F32 backward_vol_2);
/*===============================================[ private datatypes ]================================================*/
struct potentiometer_vol
{
	U16 count;
	F32 last_vol;
};
/** 
*  Private variables
**/
struct input_private
{
    struct direction_switch_obj direction_switch;
    struct analog_controller_obj accelerator;

    S16 main_contactor_error_state;
    S16 iso_valve_error_state;
    S16 buzzer_error_state;
    S16 fan_error_state;
    S16 boot_anchor_cpu_m_state;
    S16 park_break_state;
    S16 lift_switch_1_state;
    S16 tilt_switch_state;
    S16 hyd_aux_1_switch_state;
    S16 lift_switch_2_state;
    S16 seat_switch_no_state;
    S16 seat_switch_nc_state;
    S16 key_switch_state;
    S16 reserve_state;

    struct interpol_u16_s16_obj adc_calibration[mcu_adc_max_chn];
    struct potentiometer_vol pote_v;
	 
    /* from PTC*/

    F32 padel_A_start_vol;
    F32 padel_A_end_vol;
    F32 padel_B_start_vol;
    F32 padel_B_end_vol;

    F32 right_steer_anglevol;
    F32 left_steer_angle_end_vol;
    F32 steer_natrual_vol;

    F32 lift_potentiometer_start_vol;
    F32 lift_potentiometer_end_vol;

    //enum  accelerator_pedal_type Option_Peda;		/**< default  1 signle pedal , 2 double pedal  */
    //enum  steerAngle_type  option_steeranglesensor;
};

/*AD value for Pt1000*/
static const U16 temperature_x1[24] =
    {
        898,
        939,
        978,
        1018,
        1056,
        1094,
        1132,
        1170,
        1207,
        1243,
        1280,
        1315,
        1351,
        1386,
        1421,
        1455,
        1489,
        1523,
        1555,
        1589,
        1621,
        1652,
        1685,
        1716,
};
static const S16 temperature_y1[24] =
    {
        -50,
        -40,
        -30,
        -20,
        -10,
        0,
        10,
        20,
        30,
        40,
        50,
        60,
        70,
        80,
        90,
        100,
        110,
        120,
        130,
        140,
        150,
        160,
        170,
        180,
};
const struct interpol_u16_s16_obj temperatur_table1 = {24, temperature_x1, temperature_y1};

/*AD value for NTC*/
static const U16 temperature_x2[21] =
    {
        118,
        146,
        181,
        225,
        285,
        359,
        456,
        575,
        743,
        946,
        1201,
        1493,
        1832,
        2178,
        2529,
        2845,
        3106,
        3307,
        3447,
        3540,
        3597,
};
static const S16 temperature_y2[21] =
    {
        150,
        140,
        130,
        120,
        110,
        100,
        90,
        80,
        70,
        60,
        50,
        40,
        30,
        20,
        10,
        0,
        -10,
        -20,
        -30,
        -40,
        -50,
};
const struct interpol_u16_s16_obj temperatur_table2 = {21, temperature_x2, temperature_y2};

struct interpol_u16_s16_obj accelerator_table;

/*===============================================[ private variables ]================================================*/
static struct input_private input;
F32 static Time;

/*===============================================[ private functions ]================================================*/
static void input_scale_analog_values(F32 *adc_voltages);
static BO lift_potentiometer_vol_deal(F32 current_vol,struct potentiometer_vol *aa);
/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/
RC input_initialize(void)
{
    pal_memzero(&input, sizeof input);

    direction_switch_construct(&input.direction_switch);

    interpol_u16_s16_construct(&accelerator_table, 10, &service->accelerator_calibration_curve_x_1, &service->accelerator_calibration_curve_y_1);

    analog_controller_construct(&input.accelerator, &accelerator_table, 500, 500);

    driver_present_initialize();

    { /* initialize calibration curves */

        U16 index = 0;

        for (index = 0; index < mcu_adc_max_chn; index++)
        {
            //interpol_u16_s16_construct( &input.adc_calibration[index], 5, (&calibration->adc0_x_1)+index*10, (&calibration->adc0_y_1)+index*10);
        }
    }

    /* from calibration */
    input.padel_A_start_vol = calibration->accelerator_1_min * div_1000;
    input.padel_A_end_vol = calibration->accelerator_1_max * div_1000;
    input.padel_B_start_vol = calibration->accelerator_2_min * div_1000;
    input.padel_B_end_vol = calibration->accelerator_2_max * div_1000;

    input.right_steer_anglevol = calibration->steer_right * div_1000;
    input.left_steer_angle_end_vol = calibration->steer_left * div_1000;
    input.steer_natrual_vol = calibration->steer_neutral * div_1000;

    input.lift_potentiometer_start_vol = calibration->lift_potentiometer_min * div_1000;
    input.lift_potentiometer_end_vol = calibration->lift_potentiometer_max * div_1000;

    return RC_SUCCESS;
}

void input_read(struct input_data_obj *p_data)
{

    p_data->hyd_aux_switch_2 = -5; /* from can */
    p_data->pedal_type =  service->option_pedal;//SINGLE_PEDAL;//se rvice->option_pedal;
    p_data->lift_option = service->lift_mode;//lift_potentiometer;//service->lift_mode;
	p_data->steer_sensor_type = service->option_steeranglesensor; 

    /* preprocessing */
    direction_switch_process(&input.direction_switch, !mcu_get_input(EN_FORWARD_IN_D_CPU_V), !mcu_get_input(EN_NEUTRAL_POSITION_IN_D_CPU_V), !mcu_get_input(EN_REVERSE_IN_D_CPU_V));
    input_scale_analog_values(p_data->adc_voltage);
    //analog_controller_process(&input.accelerator, p_data->adc_voltage[mcu_adc_Accelerator1], p_data->adc_voltage[mcu_adc_Accelerator2], p_data->adc_voltage[mcu_adc_5VExtSns] );

    /* switches and boolean information */
    p_data->lift_switch_1 = keytime_s16(&input.lift_switch_1_state, !mcu_get_input(EN_LIFT_IN_D_CPU_V));
    p_data->tilt_switch = keytime_s16(&input.tilt_switch_state, !mcu_get_input(EN_TILT_IN_D_CPU_V));
    p_data->hyd_aux_switch_1 = keytime_s16(&input.hyd_aux_1_switch_state, !mcu_get_input(EN_MAST_AUX_1_IN_D_CPU_V));
    p_data->lift_switch_2 = keytime_s16(&input.lift_switch_2_state, !mcu_get_input(EN_MAST_AUX_2_IN_D_CPU_V));
    p_data->seat_switch_1 = keytime_s16(&input.seat_switch_nc_state, !mcu_get_input(EN_SEAT_SWITCH_IN_D_CPU_V)); 
    p_data->seat_switch_2 = keytime_s16(&input.seat_switch_no_state, !mcu_get_input(EN_SEAT_SWITCH_NC_IN_D_CPU_V));
    p_data->reserve_input = keytime_s16(&input.reserve_state, !mcu_get_input(EN_RESERVE_1_IN_D_CPU_V));
    p_data->key_switch = keytime_s16(&input.key_switch_state, p_data->adc_voltage[mcu_adc_KeySwitch] > 9.0f);
    p_data->main_contactor_error = keytime_s16(&input.main_contactor_error_state, !mcu_get_input(EN_MAIN_CONTACTOR_ERROR));
    p_data->iso_valve_error = keytime_s16(&input.iso_valve_error_state, !mcu_get_input(EN_CUT_OFF_ISO_VALVE_ERROR));
    p_data->buzzer_error = keytime_s16(&input.buzzer_error_state, !mcu_get_input(EN_BUZZER_ERROR));
    p_data->fan_error = keytime_s16(&input.fan_error_state, !mcu_get_input(EN_FAN_ERROR));
    p_data->boot_anchor_cpu_m = keytime_s16(&input.boot_anchor_cpu_m_state, !mcu_get_input(EN_BOOT_ANCHOR_CPU_M));
    p_data->park_break_switch = keytime_s16(&input.park_break_state, !mcu_get_input(EN_PARK_BRAKE_IN_D_CPU_V));
    p_data->motor_speed = mcu_get_motor_speed(); //trac motor speed

    p_data->truck_speed =(S16)((F32)fabsf(output_data.te_vSen) * ((F32)Traction.Parameter.Wheel_Radius * 1.015F * can_resol_length) / (specs->radius + 0.001f));

    /* signal pedal*/

    if (p_data->pedal_type == SINGLE_PEDAL)
    {
        p_data->direction = input.direction_switch.current_state;          //direction_switch_get_state(&input.direction_switch, NIL );
        p_data->direction_error = input.direction_switch.current_error;    //direction_switch_get_error(&input.direction_switch, NIL );
        p_data->accelerator = analog_calculate_ratio(p_data->adc_voltage[mcu_adc_Accelerator1], input.padel_A_start_vol, input.padel_A_end_vol);
    }
    else if (p_data->pedal_type == DUAL_PEDAL)
    {
        double_pedal_function( p_data,input.padel_A_start_vol,input.padel_A_end_vol,input.padel_B_start_vol,input.padel_B_end_vol);
    }
    /*  steer */
    if (p_data->steer_sensor_type == SINGLE_CHANNEL_ANGLE_SENSOR)
    {
        analog_calculate_ratio_2(p_data->adc_voltage[mcu_adc_SteerAngleA], input.steer_natrual_vol, input.right_steer_anglevol, input.left_steer_angle_end_vol, &p_data->steer);
    }
    else if (p_data->steer_sensor_type == DUAL_CHANNEL_ANGLE_SENSOR)
    {
    }
    /* lift potentiometer */
    if (p_data->lift_option == lift_potentiometer)
    {
        if(RC_SUCCESS == lift_potentiometer_vol_deal( p_data->adc_voltage[mcu_adc_lift_Pot],&input.pote_v))
        {
        	p_data->lift_potentiometer_ratio = analog_calculate_ratio(p_data->adc_voltage[mcu_adc_lift_Pot], input.lift_potentiometer_start_vol, input.lift_potentiometer_end_vol);
        }
        else
        {   if(p_data->lift_potentiometer_ratio > 0)
        	{
        		p_data->lift_potentiometer_ratio --;
        	}
        }
    }
    p_data->temperature_motor_1 = interpol_u16_s16(&temperatur_table1, (U16)mcu_get_adc(mcu_adc_TempMotInv1)); /* traction_temperature */
    p_data->temperature_motor_2 = interpol_u16_s16(&temperatur_table2, (U16)mcu_get_adc(mcu_adc_TempMotInv2)); /* pump_temperature     */
    p_data->driver_present = driver_present_process(p_data->seat_switch_1, p_data->seat_switch_2, p_data->accelerator, service->seat_switch_dual_channel);

    p_data->battery = 100;
    /* can signals */
    //p_data->traction_sensor_speed_A = traction_motor_speed_in_cpu_m_s16;

    Time = middle_f32(0.0f, Time+TS, 100000000.0f); /* ca 30000 h */
    
}

static void input_scale_analog_values(F32 *adc_voltages)
{   /* scale all adc to voltage */
    U16 index = 0;
    for (index = 0; index < mcu_adc_max_chn; index++)
    {
        adc_voltages[index] = mcu_get_adc(index);
    }
}

S16 analog_calculate_ratio(F32 input_accelerator_voltage, F32 min, F32 max)
{
    S16 rv = 0;
    F32 d_value_1 = 0;
    F32 d_value_2 = 0;
    input_accelerator_voltage = middle_f32(min, input_accelerator_voltage, max);
    if(max > min)
    {
    	d_value_1 = max - min;
		if (d_value_1 <0.01)
        {
        	rv = 0;
         }
	    else
        {
        	d_value_2 = input_accelerator_voltage - min;
        	rv = (d_value_2 / d_value_1) * 100;
    	}
    }
	else
    {
       d_value_1 = min - max;
	   if (d_value_1 <0.01)
       {
          rv = 0;
       }
	   else
       {
          d_value_2 = min - input_accelerator_voltage ;
          rv = (d_value_2 / d_value_1) * 100;
       }   
	}
	
    return rv;
}

S16 analog_calculate_ratio_2(F32 steer_A_vol, F32 natrual, F32 right_vol, F32 left_vol, struct steer_data *steer)
{
    S16 rv = 0;
    S16 angle_tmp = 0;
    if (steer_A_vol = natrual)
    {
        steer->angle = 0;
        steer->direct = steer_natrual;
    }
    else if (steer_A_vol > natrual) /*steer right*/
    {
        if ((right_vol - natrual) != 0)
        {
            steer->angle = 90 * (steer_A_vol - natrual) / (right_vol - natrual);
            steer->direct = steer_right;
        }
    }
    else if (steer_A_vol < natrual) /*steer right*/
    {
        if ((left_vol - natrual) != 0)
        {
            steer->angle = 90 * (steer_A_vol - natrual) / (left_vol - natrual);
            steer->direct = steer_left;
        }
    }
    return rv;
}

/*
*
*  function:  double_pedal_function
*  decription:                      
*      
**/
S16 double_pedal_function( struct input_data_obj *p_data,F32 forward_vol_1,F32 forward_vo2,F32 backward_vol_1,F32 backward_vol_2)
{   
   
	S16  accelerator_temp_1=0;
	S16  accelerator_temp_2=0;

	if(p_data->adc_voltage[mcu_adc_Accelerator1] > forward_vol_1)
	{
		accelerator_temp_1  = analog_calculate_ratio(p_data->adc_voltage[mcu_adc_Accelerator1],forward_vol_1,forward_vo2);  /*  accelerator_temp_1 > 2.5 ,forward   */
	}
    else if(p_data->adc_voltage[mcu_adc_Accelerator1] < backward_vol_1)
    {
    	
		accelerator_temp_2	= analog_calculate_ratio(p_data->adc_voltage[mcu_adc_Accelerator1],backward_vol_1,backward_vol_2);  /*  accelerator_temp_2 < 2.5 ,backward  */
	}
	
	if((accelerator_temp_1!= 0)&&(accelerator_temp_2 !=0))
    {
        p_data->direction_error = direction_switch_error_fw_and_bw_acktive;   //foward and backward
    }
	else if(accelerator_temp_1!= 0)
	{
		p_data->direction      = direction_switch_state_forward;
		p_data->accelerator    = accelerator_temp_1; 	
	}
	else if(accelerator_temp_2 !=0)
	{
		p_data->direction      = direction_switch_state_reverse;
		p_data->accelerator    = accelerator_temp_2; 
	}
	else
	{
		p_data->direction      = direction_switch_state_neutral;
		p_data->accelerator    = 0; 
	}
   return 0;
}

static BO lift_potentiometer_vol_deal(F32 current_vol,struct potentiometer_vol *aa)
{
    
    if((aa->last_vol >= current_vol-0.2)&&(aa->last_vol <= current_vol + 0.2))
    {
		if((aa->count++) > 5)
		{
            return RC_SUCCESS;
		}
    }
	else
	{
	    aa->last_vol = current_vol;
		aa->count =0;
	}
	return RC_ERROR;
}

__inline F32 in_Time(VO)  { return Time; }


/*===============================================[ private functions ]================================================*/

/***********************************************************************************************************************
*                             Copyright 2019 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/
